#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <boost/thread/thread.hpp>
#include<fstream>


using namespace std;

void help(char *appname) {
    printf("输入数据和平面方程，输出过滤的文件\n");
    printf("Usage: %s <input_file.txt> <plane.txt> output_folder\n", appname);

    printf("ex: %s sph1_after.txt plane1.txt output_folder\n", appname);
}

int main(int argc, char *argv[])
{
    if (argc < 4) {
        help(argv[0]);
        return -1;
    }
    string inputPointsTxt = argv[1];
    string inputPlaneTxt = argv[2];
    string outputFolder = argv[3];

    // 创建输出文件
    if (0 != access(outputFolder.c_str(), W_OK)) {
        mkdir(outputFolder.c_str(), 0777);
    }

    int n = 0; //n用来计文件中点个数
    FILE* fpInput = nullptr;

    //将点云读入并赋给新建点云指针的xyz
    double x, y, z;
    double r, g, b;

    //过滤平面噪点
    fpInput = fopen(inputPointsTxt.c_str(), "r");
    fstream f;
    f.open(inputPlaneTxt, ios::in);//打开文件，供读
    double coe_a, coe_b, coe_c, coe_d, offset;
    f >> coe_a >> coe_b >> coe_c >> coe_d >> offset;               //读取数据
    f.close();
    cout << "plane1 (A, B, C, D):" << coe_a << ", " << coe_b << ", " << coe_c << ", " << coe_d << endl;
    float val = offset;
    cout << "offset: " << offset << endl;
    while (6 == fscanf(fpInput, "%lf %lf %lf %lf %lf %lf\n", &x, &y, &z, &r, &g, &b)) {
        if (coe_a * x + coe_b * y + coe_c * z + coe_d < val) {
            ++n;
        }
    }
    cout << n << " points has been saved." << endl;
    fclose(fpInput);


    // 读取文件
    pcl::PointCloud<pcl::PointXYZRGB> cloud;
    cloud.width = n;
    cloud.height = 1;
    cloud.is_dense = false;
    cloud.points.resize(cloud.width * cloud.height);
    //新建一个点云文件，然后将结构中获取的xyz值传递到点云指针cloud中。
    fpInput = fopen(inputPointsTxt.c_str(), "r");
    int i = 0;
    while (6 == fscanf(fpInput, "%lf %lf %lf %lf %lf %lf\n", &x, &y, &z, &r, &g, &b)) {
        if (coe_a * x + coe_b * y + coe_c * z + coe_d < val) {
            cloud.points[i].x = x * 0.001; // 转成m
            cloud.points[i].y = y * 0.001;
            cloud.points[i].z = z * 0.001;
            cloud.points[i].r = r;
            cloud.points[i].g = g;
            cloud.points[i].b = b;
            cloud.points[i].a = 1; // 没弄清楚为什么a要设为1
            ++i;
        }
    }
    fclose(fpInput);

    if (n == 0) {
        cerr << "not data is found." << endl;
        return -1;
    }
    //将点云指针指向的内容传给pcd文件
    string outputFile = outputFolder + "/points1.pcd";
    cout << "save the above data of the plane to " << outputFile << endl;
    pcl::io::savePCDFileASCII(outputFile, cloud);

#if 1
    //过滤点云噪点
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered2(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr target(new pcl::PointCloud<pcl::PointXYZRGB>);
    target = cloud.makeShared();
    //string file3 = outputFolder + "/points2.pcd";
    //pcl::io::savePCDFileASCII(file3, *target);
    //pcl::io::loadPCDFile(file3, *target);
    if (target->empty()) // 使用empty()函数判断点云是否加载成功
    {
        cout << "请确认点云文件名称是否正确" << endl;
        return -1;
    }
    else
    {
        cout << "从目标点云读取 " << target->size() << " 个点" << endl;
    }

    // build the filter
    //创建过滤器
    pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> outrem;
    outrem.setInputCloud(target);        //设置输入点云
    outrem.setRadiusSearch(0.0005);        //设置在0.8的半径范围内找近邻点
    outrem.setMinNeighborsInRadius(10);  //设置查询查询点的近邻点集数小于2的删除
    outrem.filter(*cloud_filtered);     //执行滤波，结果保存在cloud_filter,apply filter
    ///*
    pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> outrem2;
    outrem2.setInputCloud(cloud_filtered);        //设置输入点云
    outrem2.setRadiusSearch(0.001);        //设置在0.8的半径范围内找近邻点
    outrem2.setMinNeighborsInRadius(50);  //设置查询查询点的近邻点集数小于2的删除
    outrem2.filter(*cloud_filtered2);     //执行滤波，结果保存在cloud_filter,apply filter
    //*/


    // display pointcloud after filtering
    std::cerr << "Cloud after filtering: " << std::endl;
    cout << "last point number: " << cloud_filtered2->size() << endl;
    string file4 = outputFolder + "/points_filter.pcd";
    pcl::io::savePCDFileASCII(file4, *cloud_filtered2);
#endif
    return 0;
}
